#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    ld = LaunchDescription()
    pkg_ros_ign = get_package_share_directory('ros_ign_gazebo')
    pkg_mecanumbot = get_package_share_directory('mecanumbot')
    sdf = DeclareLaunchArgument('sdf_file', default_value=
                                            os.path.join(pkg_mecanumbot, 'description', 'mecanum_drive.sdf'))
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_ign, 'launch', 'ign_gazebo.launch.py')
        ), 
        launch_arguments={
            'ign_args': '-r '+os.path.join(pkg_mecanumbot, 'description', 'mecanum_drive.sdf')
            #'ign_args': '-r '+os.path.join('wheeltec_robot_urdf', 'urdf', 'mini_mec_moveit_six.urdf')
        }.items(),
    )
    bridge = Node(
        package='ros_ign_bridge', 
        executable='parameter_bridge', 
        arguments=['/model/mecanumbot/cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist']
    )
    ld.add_action(gazebo)
    ld.add_action(bridge)
    return ld

